import TinderPy
import socket
import json
import numpy as np

import settings
from math import radians, cos, sin, degrees, atan2, asin

cnt = 0
speeds = np.zeros(settings.agent_num, dtype=float)
headings = np.zeros(settings.agent_num, dtype=float)

contact_speed = 0
contact_heading = 0


def is_undefined(var_name):
    global_vars = globals()
    return var_name not in global_vars


def write_log(str):

    with open('.\log.txt', 'w+') as file:
        file.write(str+'\n')


def __prepared__():
    global contact, client
    # 只在第一轮推演初始化时，建立socket连接

    # obs = [cnt]
    # for ship in ships:
    #     obs.append(ship.get_lon())
    #     obs.append(ship.get_lat())
    #     obs.append(ship.get_speed())
    #     obs.append(ship.get_heading())
    # contacts = TinderPy.get_blue_forces()
    # for contact in contacts:
    #     obs.append(contact.get_lon())
    #     obs.append(contact.get_lat())
    #     break
    # send_obs(obs)
    write_log("开始新的场景")
    TinderPy.log_info("开始新的场景")
    TinderPy.log_event("开始新的场景")
    contacts = TinderPy.get_blue_forces()
    contact = contacts[0]
    client = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    client.connect((settings.host, settings.port))
    write_log("与服务端链接成功")
    TinderPy.log_info("与服务端链接成功")
    TinderPy.log_event("与服务端链接成功")
    # 发送一个数组0，意味着reset


def send_obs(obs=[]):
    global client
    json_data = json.dumps(obs)  # 序列化要发送的数据
    print("send")
    print(json_data)
    client.sendall(json_data.encode())
    # client.close()


def recv_data():
    global client
    # client = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    # client.connect((settings.host, settings.port))
    data = client.recv(1024).decode()
    print("recv")
    print(data)
    # client.close()
    real_data = json.loads(data)
    return real_data


def conduct_control(ship_id, speed, heading):
    distance = speed * 10
    heading = radians(heading)
    desireX = distance * cos(heading)
    desireY = distance * sin(heading)
    dic = {
        "pathway": {
            "desireX": desireX,
            "desireY": desireY,
            "desireZ": 0.0,
            "timeStep": 10
        }
    }
    print(dic)
    arg = json.dumps(dic)
    # print(arg)
    TinderPy.device_control(ship_id, 2, arg)


def __next_step__(time, step):
    global client, speeds, headings, contact
    # 在推演若干步后，打印信息
    real_step = int(time / step)
    if real_step == 0:
        for i in range(6):
            TinderPy.accelerate()
        write_log("场景加速完成")
        TinderPy.log_info("场景加速完成")
        TinderPy.log_event("场景加速完成")

    # 第0步长：由于reset已经传输数据，所以无需数据
    # 向训练端发送状态计算所需的数据
    obs = []
    ships = TinderPy.get_red_forces()
    for ship in ships:
        obs.append(ship.get_lon())
        obs.append(ship.get_lat())
        obs.append(ship.get_speed())
        obs.append(ship.get_heading())
    obs.append(contact.get_lon())
    obs.append(contact.get_lat())
    # 初始状态数据的设置，设置速度和面向目标
    # if real_step <= 30:
    #     speed = 30
    #     distance = speed * 100
    #     for ship in ships:
    #         heading = get_degree(ship.get_lat(), ship.get_lon(), contact.get_lat(), contact.get_lon())
    #         conduct_control(ship.get_id(), 30, heading)
    #     return

    send_obs(obs)

    # 首先接收是否到达终止状态

    # 其次接收动作数据
    actions = recv_data()
    for i in range(settings.agent_num):
        # 进行timesteps步数后，才更新当前的动作，否则一直按原来的动作执行
        # if real_step % settings.timesteps == 0:
        speeds[i] = actions[i][0]
        headings[i] = actions[i][1]
        conduct_control(ships[i].get_id(), speeds[i], headings[i])
        # 下列为任务调度控制方向移动，速度无法控制，默认巡航速度
        # plan = TinderPy.PlanInfo()
        # plan.id = i
        # plan.force_id = ships[i].get_id()
        # distance = speed * 1000
        # dic = get_point_with_point_bearing_distance(ships[i].get_lat(), ships[i].get_lon(),
        #                                             heading, distance / 1852)
        # point = TinderPy.LLA()
        # point.lat = dic["latitude"]
        # point.lon = dic["longitude"]
        # if real_step % 200 == 0:
        #     print("lon: "+str(point.lon))
        #     print("lat: "+str(point.lat))
        # plan.points = [point]
        # plan.duration = 1000
        # plan.task_type = "1001"
        # TinderPy.conduct(plan)

    # 控制逃逸USV的移动
    conduct_control(contact.get_id(), actions[-1][0], actions[-1][1])

    done = recv_data()
    if real_step == 0:
        write_log("第0步开始")
        TinderPy.log_info("第0步开始")
        TinderPy.log_event("第0步开始")
    if done:
        print("------\n围捕成功！\n------")
        TinderPy.terminate()


def get_point_with_point_bearing_distance(lat, lon, bearing, distance):
    """
    功能：已知一点求沿某一方向一段距离的点
    :param lat:纬度
    :param lon:经度
    :param bearing:朝向角，正北为0， 顺时针依次增大
    :param distance:距离, 海里
    :return:
    """
    radius_earth_kilometres = 3440
    initial_bearing_radians = radians(bearing)
    dis_ratio = distance / radius_earth_kilometres
    dist_ratio_sine = sin(dis_ratio)
    dist_ratio_cosine = cos(dis_ratio)
    start_lat_rad = radians(lat)
    start_lon_rad = radians(lon)
    start_lat_cos = cos(start_lat_rad)
    start_lat_sin = sin(start_lat_rad)
    end_lat_rads = asin((start_lat_sin * dist_ratio_cosine) +
                        (start_lat_cos * dist_ratio_sine * cos(initial_bearing_radians)))
    end_lon_rads = start_lon_rad + atan2(sin(initial_bearing_radians) * dist_ratio_sine * start_lat_cos,
                                         dist_ratio_cosine - start_lat_sin * sin(end_lat_rads))
    my_lat = degrees(end_lat_rads)
    my_lon = degrees(end_lon_rads)
    dic = {"latitude": my_lat, "longitude": my_lon}
    return dic


def get_degree(latA, lonA, latB, lonB):
    """
    获得AB朝向与正北方向的夹角
    :param latA: A点的纬度
    :param lonA: A点的经度
    :param latB: B点的纬度
    :param lonB: B点的经度
    :return:
    """
    radLatA = radians(latA)
    radLonA = radians(lonA)
    radLatB = radians(latB)
    radLonB = radians(lonB)
    dLon = radLonB - radLonA
    y = sin(dLon) * cos(radLatB)
    x = cos(radLatA) * sin(radLatB) - sin(radLatA) * cos(radLatB) * cos(dLon)
    brng = degrees(atan2(y, x))
    brng = (brng + 360) % 360
    return brng


def __finished__():
    pass
